﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using UnityEngine.IO;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;
using Microsoft.Xna.Framework;
using UnityEngine.Graphics;
using UnityEngine.Primitive;
using MdxLib.Primitives;
using UnityEngine.Gameplay;

namespace UnityEngine.Terrain
{
    public class WarCraftMapPath
    {
        private const float GRID_SIZE = 32.0f;

        private Landscape _Landscape = null;

        public WarCraftMapPath(Landscape owner, WCLoader loader)
        {
            this._Landscape = owner;

            loader.ExpectTag("MP3W");

            Version = loader.ReadInt32();
            width = loader.ReadInt32();
            height = loader.ReadInt32();
            Debug.Assert(Version == 0);
            Debug.Assert(width == 128 * 4);
            Debug.Assert(height == 128 * 4);

            grids = new GridInformation[width * height];
            for (int i = 0; i < width; i++)
            {
                for (int j = 0; j < height; j++)
                {
                    grids[Index(i, j)] = new GridInformation(this);
                    grids[Index(i, j)].Data = loader.ReadByte();
                }
            }
        }

        public void SaveToBmp(string fileName)
        {
            string path = IOManager.Instance.GetAssertPath(fileName);

            // generate data
            int[] data = new int[width * height];
            for (int i = 0; i < width; i++)
                for (int j = 0; j < height; j++)
                    data[Index(i, j)] = grids[Index(i, j)].Data;

            // move data to bitmap storage
            using (Bitmap bmp = new Bitmap(width, height, PixelFormat.Format8bppIndexed))
            {
                BitmapData bmpData = bmp.LockBits(new System.Drawing.Rectangle(0, 0,
                                                 bmp.Width,
                                                 bmp.Height),
                                   ImageLockMode.WriteOnly,
                                   bmp.PixelFormat);

                Marshal.Copy(data, 0, bmpData.Scan0, data.Length);

                bmp.UnlockBits(bmpData);

                bmp.Save(path);
            }
        }

        private int Index(int i, int j)
        {
            return (i * width + j);
        }

        public GridInformation this[int i, int j]
        {
            get
            {
                if (i < 0 || j < 0 || i >= width || j >= height)
                    return null;
                int index = Index(i, j);
                return grids[index];
            }
        }

        public void SetTree(Tree tree)
        {
            float x = (tree.Position.Y - _Landscape.WarCraftEnvironment.CenterOffsetY) / GRID_SIZE;
            float y = (tree.Position.X - _Landscape.WarCraftEnvironment.CenterOffsetX) / GRID_SIZE;

            if (tree.IsTree)
            {
                int px = (int)x;
                int py = (int)y;

                this[px - 1, py - 1].Data |= 1;
                this[px - 1, py].Data |= 1;
                this[px, py - 1].Data |= 1;
                this[px, py].Data |= 1;
            }
        }

        public void SetDynamicCollision(Vector3 position, int size = 1, EGridType type = EGridType.IsDynamicWalkBlocker, bool isRemove = false)
        {
            float x = position.X / GRID_SIZE;
            float y = position.Z / GRID_SIZE;

            // set dynamic collision
            int px = (int)x;
            int py = (int)y;

            for (int i = -size; i <= size; i++)
                for (int j = -size; j <= size; j++)
                {
                    if (this[px + i, py + j] == null)
                        continue;
                    if (isRemove == false)
                        this[px + i, py + j].Data |= (1 << (int)type);
                    else
                    {
                        if ((this[px + i, py + j].Data & (1 << (int)type)) != 0)
                            this[px + i, py + j].Data -= (1 << (int)type);
                    }
                    this[px + i, py + j].UpdateWalkBlocker();
                }

            for (int i = -size - OnSurfaceObject.MaxDynamicCollisizeSize - 1; i <= size + OnSurfaceObject.MaxDynamicCollisizeSize + 1; i++)
                for (int j = -size - OnSurfaceObject.MaxDynamicCollisizeSize - 1; j <= size + OnSurfaceObject.MaxDynamicCollisizeSize + 1; j++)
                    PretestWalkBlocker(px + i, py + j);
        }

        #region Search Paths
        private readonly bool DrawDebugPoints = false;

        private int Sign(double value)
        {
            if (Math.Abs(value) < 0.001f) return 0;
            if (value > 0) return 1;
            if (value < 0) return -1;
            return 0;
        }

        public void PrepareForPathSearch()
        {
            for (int i = 0; i < width; i++)
                for (int j = 0; j < height; j++)
                    this[i, j].UpdateWalkBlocker();

            for (int i = 0; i < width; i++)
                for (int j = 0; j < height; j++)
                    PretestWalkBlocker(i, j);
        }

        public void PretestWalkBlocker(int x, int y)
        {
            GridInformation grid = null;
            bool blocker = false;
            int collisionSize;
            if (this[x, y] == null)
                return;
            blocker = this[x, y].IsWalkBlocker(0);
            for (collisionSize = 1; collisionSize <= OnSurfaceObject.MaxDynamicCollisizeSize; collisionSize++)
            {
                if (blocker)
                    break;
                for (int i = -collisionSize; i <= collisionSize; i++)
                {
                    grid = this[x - collisionSize, y + i];
                    if (grid != null && grid.IsWalkBlocker(0))
                    {
                        blocker = true;
                        break;
                    }
                }
                if (blocker)
                    break;
                for (int i = -collisionSize; i <= collisionSize; i++)
                {
                    grid = this[x + collisionSize, y + i];
                    if (grid != null && grid.IsWalkBlocker(0))
                    {
                        blocker = true;
                        break;
                    }
                }
                if (blocker)
                    break;
                for (int i = -collisionSize; i <= collisionSize; i++)
                {
                    grid = this[x + i, y + collisionSize];
                    if (grid != null && grid.IsWalkBlocker(0))
                    {
                        blocker = true;
                        break;
                    }
                }
                if (blocker)
                    break;
                for (int i = -collisionSize; i <= collisionSize; i++)
                {
                    grid = this[x + i, y - collisionSize];
                    if (grid != null && grid.IsWalkBlocker(0))
                    {
                        blocker = true;
                        break;
                    }
                }
                if (blocker)
                    break;
                this[x, y].SetWalkBlocker(collisionSize, false);
            }

            if (blocker == true)
            {
                for (int i = collisionSize; i <= OnSurfaceObject.MaxDynamicCollisizeSize; i++)
                {
                    this[x, y].SetWalkBlocker(i, true);
                }
            }
        }

        private bool IsValidForWalk(int x, int y, int collisionSize)
        {
            if (this[x, y] == null)
                return false;
            return !this[x, y].IsWalkBlocker(collisionSize);
        }

        private float Distance(int x, int y, int tx, int ty)
        {
            return (float)Math.Sqrt((x - tx) * (x - tx) + (y - ty) * (y - ty));
        }

        private float AreaValue(int nx, int ny, int sx, int sy, int gx, int gy)
        {
            float dx1 = nx - gx;
            float dy1 = ny - gy;
            float dx2 = sx - gx;
            float dy2 = sy - gy;
            float cross = Math.Abs(dx1 * dy2 - dy1 * dx2);
            return cross;
        }

        private float AbsSinValue(int nx, int ny, int sx, int sy, int gx, int gy)
        {
            float dx1 = nx - gx;
            float dy1 = ny - gy;
            float dx2 = sx - gx;
            float dy2 = sy - gy;
            float dis1 = this.Distance(nx, ny, gx, gy);
            float dis2 = this.Distance(sx, sy, gx, gy);
            float value;
            if (Math.Abs(dis1) < 0.01f || Math.Abs(dis2) < 0.01f)
                value = 0;
            else
                value = Math.Abs(dx1 * dy2 - dy1 * dx2) / dis1 / dis2;
            return value;
        }

        private float CosValue(int nx, int ny, int sx, int sy, int gx, int gy)
        {
            float dx1 = nx - gx;
            float dy1 = ny - gy;
            float dx2 = sx - gx;
            float dy2 = sy - gy;
            float dis1 = this.Distance(nx, ny, gx, gy);
            float dis2 = this.Distance(sx, sy, gx, gy);
            float value;
            if (Math.Abs(dis1) < 0.01f || Math.Abs(dis2) < 0.01f)
                value = 1;
            else
                value = Math.Abs(dx1 * dx2 + dy1 * dy2) / dis1 / dis2;
            return value;
        }

        private float AngleValue(int nx, int ny, int sx, int sy, int gx, int gy)
        {
            float pi = (float)Math.PI;
            float pi2 = 2.0f * (float)Math.PI;
            float dx1 = nx - gx;
            float dy1 = ny - gy;
            float dx2 = sx - gx;
            float dy2 = sy - gy;
            float angle1 = (float)Math.Atan2(dx1, dy1);
            float angle2 = (float)Math.Atan2(dx2, dy2);
            while (angle1 < 0) angle1 += pi2;
            while (angle2 < 0) angle2 += pi2;
            while (angle1 >= pi2) angle1 -= pi2;
            while (angle2 >= pi2) angle2 -= pi2;
            float differ = angle1 - angle2;
            if (differ > pi) differ -= pi;
            if (differ < -pi) differ += pi;
            if (differ < 0)
                return -differ;
            else
                return differ;
        }

        private float HeapHFunction(int nx, int ny, int sx, int sy, int gx, int gy, int method = 0)
        {
            double h_diagonal, h_min, h_max, h, h_straight;
            switch (method)
            {
                case -2:
                    return 3.0f * (float)Math.Sqrt((nx - gx) * (nx - gx) + (ny - gy) * (ny - gy));
                case -1:
                    h_diagonal = Math.Min(Math.Abs(nx - gx), Math.Abs(ny - gy));
                    h_min = Math.Max(Math.Abs(nx - gx), Math.Abs(ny - gy));
                    h_max = Math.Min(Math.Abs(nx - gx), Math.Abs(ny - gy));
                    h = 1.414 * h_diagonal + h_max - h_min;
                    return (float)h;
                case 0:
                    //h_diagonal(n) = min(abs(n.x-goal.x), abs(n.y-goal.y))
                    //h_straight(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
                    //h(n) = D2 * h_diagonal(n) + D * (h_straight(n) - 2*h_diagonal(n)))
                    h_diagonal = Math.Min(Math.Abs(nx - gx), Math.Abs(ny - gy));
                    h_straight = Math.Abs(nx - gx) + Math.Abs(ny - gy);
                    h = 1.414 * h_diagonal + 1 * (h_straight - 2 * h_diagonal);
                    return (float)h;
                case 1:
                    //dx1 = current.x - goal.x
                    //dy1 = current.y - goal.y
                    //dx2 = start.x - goal.x
                    //dy2 = start.y - goal.y
                    //cross = abs(dx1*dy2 - dx2*dy1)
                    //heuristic += cross*0.001
                    float dx1 = nx - gx;
                    float dy1 = ny - gy;
                    float dx2 = sx - gx;
                    float dy2 = sy - gy;
                    float cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                    return cross;
                case 2:
                    return Math.Max(Math.Abs(nx - gx), Math.Abs(ny - gy));
                case 3:
                    return Math.Abs(nx - gx) + Math.Abs(ny - gy);
            }
            return 0;
        }

        private int GetDirection(HValue p, HValue c)
        {
            int x = c.X - p.X;
            int y = c.Y - p.Y;
            return (x + 1) * 3 + y;
        }

        private List<Vector3> BuildPath(List<HValue> paths, Vector3 start, Vector3 end, int collisionSize, Microsoft.Xna.Framework.Color color)
        {
            List<Vector3> ret = new List<Vector3>();
            ret.Clear();

            // Collapse the paths in two steps:
            // step 1:
            // Collapse points that are in the same direction
            List<HValue> newPaths = new List<HValue>();
            for (int i = 0; i < paths.Count; i++)
            {
                bool add = true;
                if (0 < i && i + 1 < paths.Count)
                {
                    int d1 = GetDirection(paths[i - 1], paths[i]);
                    int d2 = GetDirection(paths[i], paths[i + 1]);
                    if (d1 == d2) add = false;
                    add = true;
                }
                if (add) newPaths.Add(paths[i]);
            }
            paths = newPaths;
            newPaths = new List<HValue>();
            newPaths.Clear();

            // step 2:
            List<Vector3> realPaths = new List<Vector3>();
            foreach (HValue h in paths)
            {
                realPaths.Add(new Vector3(h.X * 32.0f, 1300.0f, h.Y * 32.0f));
            }
            int count = realPaths.Count;
            if (count == 0 || Vector3.Distance(start, realPaths[0]) > 0.1f)
                realPaths.Insert(0, start);
            if (Vector3.Distance(end, realPaths[realPaths.Count - 1]) > 0.1f)
                realPaths.Add(end);

            // step 3:
            ret.Clear();
            for (int current = 0; current < realPaths.Count; )
            {
                ret.Add(realPaths[current]);
                int jumpTo = -1;
                for (int j = realPaths.Count - 1; j > current; j--)
                {
                    Vector3 s = realPaths[current];
                    Vector3 t = realPaths[j];
                    s.Y = t.Y = 0;
                    float actualMoveDis = this.TryWalkDistance(s, t, collisionSize);
                    if (Math.Abs(Vector3.Distance(s, t) - actualMoveDis) < 0.01f)
                    {
                        jumpTo = j;
                        break;
                    }
                }
                if (jumpTo == -1)
                {
                    jumpTo = current + 1;
                    //throw new Exception("Error happens when building paths");
                    //Debug.WriteLine("Error happens when building paths");
                }

                current = jumpTo;
                // reach the final position
                if (current == realPaths.Count - 1)
                {
                    ret.Add(realPaths[current]);
                    break;
                }
            }
            paths = newPaths;

            if (this.DrawDebugPoints)
            {
                // draw points for debug
                foreach (Vector3 v in ret)
                {
                    Vector3 positionOnTerrain = _Landscape.WarCraftMapPath.GetHeight(v);
                    DebugPrimitiveRenderer.AddPermanentPrimitive(positionOnTerrain, color);
                }
            }

            return ret;
        }

        // this is for the situation start point and end point are not connected.
        private HValue _NearestTarget = null;
        // two heaps are created, it will make the search more smooth.
        // from start to end, it may possibly needs very points, for example, start to end needs search 10000, but end to start search 1000
        // two heaps make the search at he average search count 5000.
        private Heap[] _Heap = new Heap[2];
        private HValue[,] _XPath = null;
        private Dictionary<int, List<Vector3>> _SpecialCaseRecord = new Dictionary<int, List<Vector3>>();
        public void RequestPaths(Vector3 startPoint, Vector3 endPoint, int collisionSize, out List<Vector3> paths, int specialCase)
        {
            if (specialCase >= 0 && _SpecialCaseRecord.ContainsKey(specialCase))
            {
                paths = _SpecialCaseRecord[specialCase];
                return;
            }

            if (collisionSize > OnSurfaceObject.MaxDynamicCollisizeSize)
                throw new Exception("Dynamic collision size is out of range");

            paths = new List<Vector3>();

            // calculate the discrete positions
            int[] heapX = new int[2];
            int[] HeapY = new int[2];
            heapX[0] = (int)UnityHelper.GetFloorValue(startPoint.X / GRID_SIZE);
            HeapY[0] = (int)UnityHelper.GetFloorValue(startPoint.Z / GRID_SIZE);
            heapX[1] = (int)UnityHelper.GetFloorValue(endPoint.X / GRID_SIZE);
            HeapY[1] = (int)UnityHelper.GetFloorValue(endPoint.Z / GRID_SIZE);
            if (heapX[0] == heapX[1] && HeapY[0] == HeapY[1])
            {
                paths.Add(endPoint);
                return;
            }

            // prepare for searching
            for (int i = 0; i < 2; i++)
            {
                if (_Heap[i] == null)
                    _Heap[i] = new Heap();
                _Heap[i].Clear();
            }
            if (_XPath == null)
            {
                _XPath = new HValue[this.width, this.height];
                for (int i = 0; i < width; i++)
                    for (int j = 0; j < height; j++)
                        _XPath[i, j] = new HValue();
            }

            for (int i = 0; i < width; i++)
                for (int j = 0; j < height; j++)
                    _XPath[i, j].Reset();

            const int markBase = 10;
            // push first point to heap
            for (int heapIndex = 0; heapIndex < 2; heapIndex++)
            {
                int sx = heapX[heapIndex];
                int sy = HeapY[heapIndex];
                int tx = heapX[1 - heapIndex];
                int ty = HeapY[1 - heapIndex];

                for (int i = -1; i <= 1; i++)
                    for (int j = -1; j <= 1; j++)
                    {
                        int x = sx + i;
                        int y = sy + j;
                        Vector3 from = (heapIndex == 0) ? startPoint : endPoint;
                        Vector3 target = new Vector3(x * 32.0f, 0.0f, y * 32.0f);
                        from.Y = target.Y = 0;
                        float distance = Vector3.Distance(from, target);
                        float canMove = this.TryWalkDistance(from, target, collisionSize);
                        if (Math.Abs(canMove - distance) < 0.01f)
                        {
                            _XPath[x, y].Set(x, y, distance, 0);
                            _XPath[x, y].Mark = 1 + heapIndex * markBase;
                            _XPath[x, y].Previous = null;
                            _XPath[x, y].Depth = 0;
                            _Heap[heapIndex].Push(_XPath[x, y]);
                        }
                    }
            }

            _NearestTarget = _Heap[0].Peek();

            //directions
            const int dC = 8;
            int[] dx = new int[dC];
            int[] dy = new int[dC];
            for (int i = 0; i < dC; i++)
            {
                float angle = i * 2.0f * (float)Math.PI / (float)dC;
                dx[i] = this.Sign(Math.Cos(angle));
                dy[i] = this.Sign(Math.Sin(angle));
            }

            List<HValue> pathHValues = new List<HValue>();
            pathHValues.Clear();
            if (this.DrawDebugPoints)
                DebugPrimitiveRenderer.Clear();
            while (_Heap[0].IsEmpty == false || _Heap[1].IsEmpty == false)
            {
                int heapIndex = 0;
                if (_Heap[0].Count < _Heap[1].Count)
                    heapIndex = 0;
                else
                    heapIndex = 1;
                if (_Heap[0].Count == 0) heapIndex = 1;
                if (_Heap[1].Count == 0) heapIndex = 0;
                {
                    HValue current = _Heap[heapIndex].Pop();
                    current.Mark = heapIndex * markBase + 2;

                    if (this.DrawDebugPoints)
                    {
                        Vector3 p = new Vector3(current.X * 32.0f, 1300.0f, current.Y * 32.0f);
                        Vector3 shouldP = _Landscape.WarCraftMapPath.GetHeight(p);
                        DebugPrimitiveRenderer.AddPermanentPrimitive(shouldP, Microsoft.Xna.Framework.Color.Orange);
                    }

                    int arrivedX, arrivedY;
                    // search each direction
                    for (int d = 0; d < dC; d++)
                    {
                        float unitCost = (float)(dx[d] * dx[d] + dy[d] * dy[d]);
                        unitCost = (float)Math.Sqrt(unitCost);

                        arrivedX = current.X + dx[d];
                        arrivedY = current.Y + dy[d];

                        // check if could arrived at the position
                        bool canArrived = true;
                        canArrived = this.IsValidForWalk(current.X, current.Y, collisionSize);// this[arrivedX, arrivedY].IsValidForWalk;
                        if (Math.Abs(unitCost - 1) < 0.01f)
                            canArrived = this.IsValidForWalk(arrivedX, arrivedY, collisionSize);//this[arrivedX, arrivedY].IsValidForWalk;
                        else
                        {
                            for (int i = 0; i <= 1; i++)
                                for (int j = 0; j <= 1; j++)
                                    canArrived = canArrived && this.IsValidForWalk(current.X + i * dx[d], current.Y + j * dy[d], collisionSize);
                        }
                        // if arrive at a valid point
                        if (canArrived)
                        {
                            if (_XPath[arrivedX, arrivedY].Mark == heapIndex * markBase + 2)
                            {
                                // do nothing
                            }
                            if (_XPath[arrivedX, arrivedY].Mark == heapIndex * markBase + 1)
                            {
                                float cost = current.Cost + unitCost;
                                if (cost + 0.1f < _XPath[arrivedX, arrivedY].Cost)
                                {
                                    float dis = this.Distance(current.X, current.Y, arrivedX, arrivedY);
                                    int pos = _Heap[heapIndex].Find(_XPath[arrivedX, arrivedY]);
                                    _XPath[arrivedX, arrivedY].Set(arrivedX, arrivedY, cost, cost + dis);
                                    _XPath[arrivedX, arrivedY].Previous = current;
                                    _XPath[arrivedX, arrivedY].Depth = current.Depth + 1;
                                    _Heap[heapIndex].Upper(pos);
                                }
                            }
                            if (_XPath[arrivedX, arrivedY].Mark == -1)
                            {
                                float cost = current.Cost + unitCost;
                                float dis = this.HeapHFunction(arrivedX, arrivedY, heapX[heapIndex], HeapY[heapIndex], heapX[1 - heapIndex], HeapY[1 - heapIndex]);
                                _XPath[arrivedX, arrivedY].Set(arrivedX, arrivedY, cost, cost + dis);
                                _XPath[arrivedX, arrivedY].Previous = current;
                                _XPath[arrivedX, arrivedY].Depth = current.Depth + 1;
                                _XPath[arrivedX, arrivedY].Mark = heapIndex * markBase + 1;
                                _Heap[heapIndex].Push(_XPath[arrivedX, arrivedY]);

                                if (this.DrawDebugPoints)
                                {
                                    Vector3 p = new Vector3(arrivedX * 32.0f, 1300.0f, arrivedY * 32.0f);
                                    Vector3 shouldP = _Landscape.WarCraftMapPath.GetHeight(p);
                                    DebugPrimitiveRenderer.AddPermanentPrimitive(shouldP, Microsoft.Xna.Framework.Color.Yellow);
                                }
                            }

                            // Record nearest point
                            if (heapIndex == 0)
                            {
                                //float area1 = this.AreaValue(_NearestTarget.X, _NearestTarget.Y, heapX[heapIndex], HeapY[heapIndex], heapX[1 - heapIndex], HeapY[1 - heapIndex]);
                                // float area2 = this.AreaValue(arrivedX, arrivedY, heapX[heapIndex], HeapY[heapIndex], heapX[1 - heapIndex], HeapY[1 - heapIndex]);
                                //float angle1 = this.AngleValue(_NearestTarget.X, _NearestTarget.Y, heapX[heapIndex], HeapY[heapIndex], heapX[1 - heapIndex], HeapY[1 - heapIndex]);
                                //float angle2 = this.AngleValue(arrivedX, arrivedY, heapX[heapIndex], HeapY[heapIndex], heapX[1 - heapIndex], HeapY[1 - heapIndex]);
                                float dis1 = this.Distance(_NearestTarget.X, _NearestTarget.Y, heapX[1], HeapY[1]);
                                float dis2 = this.Distance(arrivedX, arrivedY, heapX[1], HeapY[1]);

                                if (dis2 < dis1)
                                {
                                    _NearestTarget = _XPath[arrivedX, arrivedY];
                                }
                                float currentKey = current.Cost + dis2;
                                float NearestKey = _NearestTarget.Cost + 8 * dis1;
                                if (currentKey > NearestKey && _Heap[0].PushCount > 4096 && _Heap[1].IsEmpty)
                                //if (currentKey > NearestKey && _Heap[0].PushCount > 512 && _Heap[1].IsEmpty)
                                {
                                    // first record heap 0
                                    current = _NearestTarget;
                                    while (current != null)
                                    {
                                        pathHValues.Add(current);
                                        current = current.Previous;
                                    }
                                    // reverse it
                                    for (int i = 0; i < pathHValues.Count / 2; i++)
                                    {
                                        HValue temp = pathHValues[i];
                                        pathHValues[i] = pathHValues[pathHValues.Count - i - 1];
                                        pathHValues[pathHValues.Count - i - 1] = temp;
                                    }

                                    paths = this.BuildPath(pathHValues, startPoint, new Vector3(_NearestTarget.X * 32.0f, 0.0f, _NearestTarget.Y * 32.0f), collisionSize, Microsoft.Xna.Framework.Color.Purple);
                                    if (specialCase >= 0)
                                        this._SpecialCaseRecord.Add(specialCase, paths);
                                    return;
                                }
                            }

                            // if find grid that both meet
                            if (_XPath[arrivedX, arrivedY].Mark >= 0 && _XPath[arrivedX, arrivedY].Mark / 10 != heapIndex)
                            {
                                // record two ends
                                HValue[] ends = new HValue[2];
                                ends[heapIndex] = current;
                                ends[1 - heapIndex] = _XPath[arrivedX, arrivedY];

                                paths.Clear();
                                // first record heap 0
                                current = ends[0];
                                while (current != null)
                                {
                                    pathHValues.Add(current);
                                    current = current.Previous;
                                }
                                // reverse it
                                for (int i = 0; i < pathHValues.Count / 2; i++)
                                {
                                    HValue temp = pathHValues[i];
                                    pathHValues[i] = pathHValues[pathHValues.Count - i - 1];
                                    pathHValues[pathHValues.Count - i - 1] = temp;
                                }

                                // then record heap 1
                                current = ends[1];
                                while (current != null)
                                {
                                    pathHValues.Add(current);
                                    current = current.Previous;
                                }
                                paths = this.BuildPath(pathHValues, startPoint, endPoint, collisionSize, Microsoft.Xna.Framework.Color.Red);
                                if (specialCase >= 0)
                                    this._SpecialCaseRecord.Add(specialCase, paths);
                                return;
                            }
                        }

                    }
                }
            }
        }
        #endregion

        public bool IsValidForWalk(Vector3 position, int collisionSize)
        {
            float px = position.X / GRID_SIZE;
            float pz = position.Z / GRID_SIZE;
            int x = (int)UnityHelper.GetFloorValue(position.X / GRID_SIZE);
            int z = (int)UnityHelper.GetFloorValue(position.Z / GRID_SIZE);
            return IsValidForWalk(x, z, collisionSize);
        }

        private float Right(float x)
        {
            return UnityHelper.GetFloorValue(x + 1);
        }

        private float Left(float x)
        {
            float ret = UnityHelper.GetFloorValue(x);
            if (Math.Abs(x - ret) < 0.01f)
                return (x - 1);
            else
                return ret;
        }

        // move from start to target directly
        // return the length to the point of first blocker
        private Dictionary<int, float> _DistanceRecord = new Dictionary<int, float>();
        private List<int> _PointsList = new List<int>();
        private const int _BaseValue = 2048;
        private int GetKey(Vector3 pos)
        {
            int x = UnityHelper.GetFloorValue(pos.X);
            int y = UnityHelper.GetFloorValue(pos.Z);
            return x * _BaseValue + y;
        }

        private Microsoft.Xna.Framework.Point ToValue(int key)
        {
            return new Microsoft.Xna.Framework.Point(key / _BaseValue, key % _BaseValue);
        }

        public float TryWalkDistance(Vector3 start, Vector3 target, int collisionSize)
        {
            if (this.IsValidForWalk(start, collisionSize) == false)
                return 0.0f;

            start.Y = target.Y = 0;
            float eDis = Vector3.Distance(start, target) / GRID_SIZE;
            start = start / GRID_SIZE;
            target = target / GRID_SIZE;

            Vector3 direction = target - start;
            if (direction == Vector3.Zero)
                return 0.0f;
            direction.Normalize();

            float moveDis = 0;
            float currentDis = 0;
            Vector3 position = start;

            _DistanceRecord.Clear();
            _PointsList.Clear();
            while (currentDis <= eDis)
            {
                // find next position
                float xDis = 10000000.0f;
                float zDis = 10000000.0f;
                float testStep = 0.5f;
                if (direction.X != 0)
                {
                    if (direction.X > 0)
                    {
                        float p = Right(position.X);
                        xDis = (p - position.X) / direction.X;
                        if (xDis <= 0)
                            xDis = (p + testStep - position.X) / direction.X;
                    }
                    else
                    {
                        float p = Left(position.X);
                        xDis = (position.X - p) / -direction.X;
                        if (xDis <= 0)
                            xDis = (position.X + testStep - p) / -direction.X;
                    }
                }
                if (direction.Z != 0)
                {
                    if (direction.Z > 0)
                    {
                        float p = Right(position.Z);
                        zDis = (p - position.Z) / direction.Z;
                        if (zDis <= 0)
                            zDis = (p + testStep - position.Z) / direction.Z;
                    }
                    else
                    {
                        float p = Left(position.Z);
                        zDis = (position.Z - p) / -direction.Z;
                        if (zDis <= 0)
                            zDis = (position.Z + testStep - p) / -direction.Z;
                    }
                }

                if (xDis <= 0f || zDis <= 0f)
                    throw new Exception("Step forward should be above zero.");
                float choose = 0.0f;
                if (xDis < zDis) choose = xDis; else choose = zDis;

                moveDis = currentDis + choose / 2.0f;
                if (moveDis > eDis) moveDis = eDis;
                position = start + moveDis * direction;
                int key = GetKey(position);
                if (_DistanceRecord.ContainsKey(key) == false)
                {
                    _DistanceRecord.Add(key, currentDis);
                    _PointsList.Add(key);
                }

                moveDis = currentDis + choose;
                if (moveDis > eDis) moveDis = eDis;
                position = start + moveDis * direction;
                key = GetKey(position);
                if (_DistanceRecord.ContainsKey(key) == false)
                {
                    _DistanceRecord.Add(key, currentDis);
                    _PointsList.Add(key);
                }

                currentDis = moveDis;
                //bool pass = true;
                //moveDis = currentDis + choose / 2.0f;
                //if (moveDis > eDis) moveDis = eDis;
                //position = start + moveDis * direction;
                //pass = pass && IsValidForWalk(position * 32.0f, collisionSize);

                //moveDis = currentDis + choose;
                //if (moveDis > eDis) moveDis = eDis;
                //position = start + moveDis * direction;
                //pass = pass && IsValidForWalk(position * 32.0f, collisionSize);

                // test the position is available
                //if (pass == false)
                //    break;
                //else
                //    currentDis = moveDis;

                if (currentDis >= eDis)
                {
                    currentDis = eDis;
                    break;
                }
            }

            int LastKey = -1;
            Microsoft.Xna.Framework.Point LastPoint = new Microsoft.Xna.Framework.Point(0, 0);
            for (int i = 0; i < _PointsList.Count; i++)
            {
                int key = _PointsList[i];
                Microsoft.Xna.Framework.Point point = ToValue(key);

                if (LastKey != -1 && Math.Abs(LastPoint.X - point.X) + Math.Abs(LastPoint.Y - point.Y) == 2)
                {
                    if (IsValidForWalk(LastPoint.X, point.Y, collisionSize) == false || IsValidForWalk(point.X, LastPoint.Y, collisionSize) == false)
                    {
                        currentDis = _DistanceRecord[key];
                        break;
                    }
                }
                if (IsValidForWalk(point.X, point.Y, collisionSize) == false)
                {
                    currentDis = _DistanceRecord[key];
                    break;
                }

                LastPoint = point;
                LastKey = key;
            }

            return currentDis * 32.0f;
        }

        public Vector3 GetHeight(Vector3 position)
        {
            Vector3 result;
            this.GetHeight(position, out result);
            return result;
        }

        public void GetHeight(Vector3 position, out Vector3 positionOnTerrain)
        {
            positionOnTerrain = position;
            int x = (int)(position.X / 32.0f);
            int y = (int)(position.Z / 32.0f);

            if (x < 0 || x > width || y < 0 || y > height)
                return;

            // get position precisely
            int pX = x / 4;
            int pY = y / 4;

            if (pX < 0 || pX + 1 >= _Landscape.WarCraftEnvironment.Width || pY < 0 || pY + 1 >= _Landscape.WarCraftEnvironment.Width)
                return;

            string name = "";
            VertexInfo A = this._Landscape.WarCraftEnvironment[pX, pY];
            VertexInfo B = this._Landscape.WarCraftEnvironment[pX + 1, pY];
            VertexInfo D = this._Landscape.WarCraftEnvironment[pX + 1, pY + 1];
            VertexInfo C = this._Landscape.WarCraftEnvironment[pX, pY + 1];

            ECellType cellType = CliffPaletteRendererContainer.GetCellType(A, B, D, C, out name);

            if (true || cellType == ECellType.Surface || cellType == ECellType.Ramp)
            {
                // if in the triangle A - B - C
                if (position.X - A.Position.X + position.Z - A.Position.Z <= 128.0f)
                {
                    float h1 = (position.X - A.Position.X) / (B.Position.X - A.Position.X) * (B.Position.Y - A.Position.Y);
                    float h2 = (position.Z - A.Position.Z) / (C.Position.Z - A.Position.Z) * (C.Position.Y - A.Position.Y);

                    positionOnTerrain.Y = h1 + h2 + A.Position.Y;
                }
                else
                {
                    // in the triangle D - C - B
                    float h1 = (position.X - D.Position.X) / (C.Position.X - D.Position.X) * (C.Position.Y - D.Position.Y);
                    float h2 = (position.Z - D.Position.Z) / (B.Position.Z - D.Position.Z) * (B.Position.Y - D.Position.Y);

                    positionOnTerrain.Y = h1 + h2 + D.Position.Y;
                }
            }
            else
            {
                Debug.WriteLine("Place object on cliff/clifftrans is not supported");
                //throw new Exception("Place object on cliff/clifftrans is not supported.");
            }
        }

        public Vector3 GetRotation(Vector3 position)
        {
            Vector3 result;
            this.GetRotation(position, out result);
            return result;
        }

        public void GetRotation(Vector3 position, out Vector3 rotationOnTerrain)
        {
            rotationOnTerrain = new Vector3(0, 0, 0);
            int x = (int)(position.X / 32.0f);
            int y = (int)(position.Z / 32.0f);

            if (x < 0 || x > width || y < 0 || y > height)
                return;

            // get position precisely
            int pX = x / 4;
            int pY = y / 4;

            string name = "";
            VertexInfo A = this._Landscape.WarCraftEnvironment[pX, pY];
            VertexInfo B = this._Landscape.WarCraftEnvironment[pX + 1, pY];
            VertexInfo D = this._Landscape.WarCraftEnvironment[pX + 1, pY + 1];
            VertexInfo C = this._Landscape.WarCraftEnvironment[pX, pY + 1];

            ECellType cellType = CliffPaletteRendererContainer.GetCellType(A, B, D, C, out name);

            if (true || cellType == ECellType.Surface || cellType == ECellType.Ramp)
            {
                // if in the triangle A - B - C
                if (position.X - A.Position.X + position.Z - A.Position.Z <= 128.0f)
                {
                    float angle1 = (float)Math.Atan((B.Position.Y - A.Position.Y) / 128.0f);
                    float angle2 = (float)Math.Atan((C.Position.Y - A.Position.Y) / 128.0f);

                    rotationOnTerrain = new Vector3(-angle2, 0, angle1);
                }
                else
                {
                    float angle1 = (float)Math.Atan((C.Position.Y - D.Position.Y) / -128.0f);
                    float angle2 = (float)Math.Atan((B.Position.Y - D.Position.Y) / -128.0f);

                    rotationOnTerrain = new Vector3(-angle2, 0, angle1);
                }
            }
            else
            {
                Debug.WriteLine("Place object on cliff/clifftrans is not supported");
                //throw new Exception("Place object on cliff/clifftrans is not supported.");
            }

            //DebugPrimitiveRenderer.AddImmdiatePrimitive(positionOnTerrain, Microsoft.Xna.Framework.Color.Yellow);
            //DebugPrimitiveRenderer.AddImmdiatePrimitive(A.Position, Microsoft.Xna.Framework.Color.Red);
            //DebugPrimitiveRenderer.AddImmdiatePrimitive(B.Position, Microsoft.Xna.Framework.Color.Green);
            //DebugPrimitiveRenderer.AddImmdiatePrimitive(C.Position, Microsoft.Xna.Framework.Color.Blue);
            //DebugPrimitiveRenderer.AddImmdiatePrimitive(D.Position, Microsoft.Xna.Framework.Color.Gray);
        }

        public void GetHeightAndRotation(Vector3 position, out Vector3 positionOnTerrain, out Vector3 rotationOnTerrain)
        {
            this.GetHeight(position, out positionOnTerrain);
            this.GetRotation(position, out rotationOnTerrain);
        }

        private int Version;
        private int width;
        private int height;
        private GridInformation[] grids;
    }
}
